#ifndef _TRACKLET_
#define _TRACKLET_

// Includes:
// --------
// Standard
#include <cstdlib>
#include <ctime>

// ROS/OpenCV/PCL
#include "tf/transform_listener.h"
#include <opencv2/core/core.hpp>
#include <image_geometry/pinhole_camera_model.h>
#include <sensor_msgs/CameraInfo.h>

// Namespaces:
// ----------
using namespace std;

// -----------
class Tracklet
// -----------
{
	private:
		int age;
		int detections;

	protected:
		// ----------------------------------------------------------------------------------
		double getDeltaU(image_geometry::PinholeCameraModel cameraModel, double dX, double z)
		// ----------------------------------------------------------------------------------
		{
			cv::Point3d pt3A(0,0,z);
			cv::Point3d pt3B(dX,0,z);

			cv::Point2d pt2A, pt2B;
			cameraModel.project3dToPixel(pt3A, pt2A);
			cameraModel.project3dToPixel(pt3B, pt2B);
			return abs(pt2A.x - pt2B.x);
		}

		// ----------------------------------------------------------------------------------
		double getDeltaV(image_geometry::PinholeCameraModel cameraModel, double dY, double z)
		// ----------------------------------------------------------------------------------
		{
			cv::Point3d pt3A(0,0,z);
			cv::Point3d pt3B(0,dY,z);

			cv::Point2d pt2A, pt2B;
			cameraModel.project3dToPixel(pt3A, pt2A);
			cameraModel.project3dToPixel(pt3B, pt2B);
			return abs(pt2A.y - pt2B.y);
		}

		// --------------------
		inline double dRandom()
		// --------------------
		{
			return (double)(rand() / (double)RAND_MAX);	
		}


	public:
		// -------
		Tracklet()
		// -------
		{
			this->age = 1;
			this->detections = 1;

			// Seed the random number generator
			srand(time(0));
		}

		// Interface:
		// ------------------------------------------------
		virtual void update(geometry_msgs::PointStamped pt)
		// ------------------------------------------------
		{
			++(this->detections);	
			++(this->age);	
		}
		
		// ------------------
		virtual void update()
		// ------------------
		{
			++(this->age);
		}

		virtual cv::Rect getROI(string& cameraFrame, ros::Time t) = 0;
		virtual bool contains(double d) = 0;

		// Other
		// -----------------
		virtual int getAge()
		// -----------------
		{
			return this->age;
		}

		// ----------------
		bool killTracklet()
		// ----------------
		{
			double r = this->dRandom();
			return r <  (1.0f - ((double)(detections)/(double)age));
		}

};

#endif /* _TRACKLET_ */
